Вы можете просто добавить все предыдущие точки узлов к итогу, чтобы получить расстояние, или вы можете добавить к нему, когда юнит перемещается между узлами.
Я бы сказал, что для управления такого рода движением вам может быть трудно управлять узлами, просто выполнив pathObjects = GameObject.FindGameObjectsWithTag("PathPoint");
, было бы лучше иметь класс, который содержит их список, и в этом случае вы могли бы индексировать их. В настоящее время вы не знаете, находятся ли ваши узлы в правильном порядке.
Затем вы можете сделать то же самое для цикла, который есть в PathDistance, но просто сделать ранний выход, когда достигнете текущего индекса пути, и, наконец, добавить расстояние от единицы до последнего измеренного вами узла.