Blitzerg, как вариант вместо трианглов, в качестве вэйпоинтов, можно юзать пивоты !!
в коде ты конечно if'ом побаловался изрядно !!
Как правило, воизбижения этого юзают циклы !!
что бы не проверять каждый раз EntityDistance, можно заюзать переменную, которая будет показивать этот EntityDistance !!