Compare distance of many objects from current object

I have a waypoint system where my Player moves along a series of WPs from a starting position to an end position, in the Z-direction. The WPs are laid out in the X-Y plane like a wall of objects in a grid-like formation (9 WPs in a square 3 x 3 formation). Now, there are a number of these ‘groups’ of 9 WPs laid out at various intervals along the Z direction and the player moves though a path starting from the first to last group (the path taken based upon the WP chosen in the first group).

Now, the issue I’m having is when the Player is at the WP in the End Group (see image) and needs to move into an entirely new set of groups (into a WP in new Start Group) I would like to select the WP it starts at based on its distace from the current WP. I know the closest will be the one directly ahead of it (as opposed to one diagonally above or below, say), but I’m curious as to how to accomplish this is when quick distance comparisons might be required.

Vector3.Distance springs to mind here. Check the distance of the current WP from each WPs in the first group and move to the closest. However, this calculation must be done on the fly and I know using Vector3.Distance can be expensive, especially if having to compare a number of distances. I then thought of a Physics.Spherecast, but there are no colliders on the WPs so I’d like to avoid that if possible. I could place a sphere trigger on each of the WPs in the end group and check for the WP that enter it (WP in Start group to only have colliders), then do a Vector3.Distance on these WP only? The other option of course is to just tell it which WP to move to (Eg. if current WP is 30, move to either 2, 3 or 4, etc).

Any suggestions?



Vector3.Distance really isn’t that expensive. Unless your doing it hundreds-thousands of times per second, you probably won’t even notice. Using colliders/spherecasts will internally do a similair calculation anyway, and are probably more expensive.

All that said, the main reason that Vector3.Distance is expensive is that it involves a square-root, and if you want to know is which vector is longest, you can compare square magnitudes instead of distances, in which case all your calculating is some multiplication and addition, which is very cheap. Example code:

Vector3 ChooseNearest(Vector3 location, Vector3[] destinations)
        float nearestSqrMag = float.PositiveInfinity;
        Vector3 nearestVector3 =;

        for(int i = 0; i < destinations.Length; i++)
            float sqrMag = (destinations *- location).sqrMagnitude;*

if(sqrMag < nearestSqrMag)
nearestSqrMag = sqrMag;
nearestVector3 = destinations*;*

return nearestVector3;
Hope that helps.