public float CalculateDistFromDestination()
{
float dist = Vector3.Distance(thisT.position, subPath[subWaypointID]);
for (int i = subWaypointID + 1; i < subPath.Count; i++)
{
dist += Vector3.Distance(subPath[i - 1], subPath[i]);
}
dist += path.GetPathDistance(waypointID + 1);
return(dist);
}