MultiGoalShortestPath improvements

[aclegg3]

Motivation and Context

Currently, if any of the requested_end points don't snap to the navmesh, MultiGoalShortestPath will early abort and return inf distance.
Expected behavior: if any of the points in requested_ends are valid, the shortest path should be computed accurately.

Additional feature: adds closest_end_point_index to MultiGoalShortestPath struct API. Makes it easier to know which point form the input set was actually selected as closets. Corresponds to the end of the path.

How Has This Been Tested

TODO:
May need to update tests to check these edge cases:

• all points off navmesh
• some points off navmesh
• all points on other island
• all point son other island or off navmesh

Types of changes

• Docs change / refactoring / dependency upgrade
• Bug fix (non-breaking change which fixes an issue)
• New feature (non-breaking change which adds functionality)
• Breaking change (fix or feature that would cause existing functionality to change)

Checklist

• My code follows the code style of this project.
• My change requires a change to the documentation.
• I have updated the documentation accordingly.
• [x ] I have read the CONTRIBUTING document.
• I have completed my CLA (see CONTRIBUTING)
• I have added tests to cover my changes.
• All new and existing tests passed.

[jturner65]

LGTM!