Sprawdzanie najkrótszej trasy

Witam serdecznie :)
Problem z którym się borykam wygląda następująco: piszę funkcję, która (w założeniu) ma wyszukiwać najkrótszą trasę, bazując na wyznaczonej ścieżce. Liczyć trasę liczy, ale kiedy przychodzi do sprawdzenia czy która jest krótsza to tego nie robi. Trochę zagmatwałem, może lepiej przedstawię kod:

  void SzukajNajblizszej() // wyszukiwanie nowego targetu po najblizszej sciezce
    {
        licznikSPR = 0;
        GameObject[] towery;
        towery = GameObject.FindGameObjectsWithTag("Tower");
        float najkrotszaOdleglosc = 0f;
        target = towery[0];
        bool pierwszy = true;
        float trasa;
        float[] tablicaSpr = new float[3];
        for(int i = 0; i < towery.Length;i++)
        {
            if(towery[i].GetComponent<TowerController>().playerWlasciciel)
            {
                navMesh.SetDestination(towery[i].transform.position);
                
                if(pierwszy)
                {
                    najkrotszaOdleglosc = PathLength(navMesh.path);
                    pierwszy = false;
                    
                }
                trasa = PathLength(navMesh.path);
                tablicaSpr[i] = trasa;
               if(najkrotszaOdleglosc > trasa)
                {
                    
                    najkrotszaOdleglosc = trasa;
                    target = towery[i];
                    licznikSPR += 1;

                }//if (najkrotszaOdleglosc > navMesh.remainingDistance)

            }//if(towery[i].GetComponent<TowerController>().playerWlasciciel)
            licznikSPR += 1;
        }//for(int i = 1; i < towery.Length;i++)
        najkotsza = najkrotszaOdleglosc;
        navMesh.SetDestination(target.transform.position);
        sprfloat = tablicaSpr;

    }// void SzukajNajblizszej()

funkcję liczącą długość ścieżki wziąłem ze strony Unity:

 
float PathLength(NavMeshPath path)
    {
        if (path.corners.Length < 2)
            return 0;

        Vector3 previousCorner = path.corners[0];
        float lengthSoFar = 0.0F;
        int i = 1;
        while (i < path.corners.Length)
        {
            Vector3 currentCorner = path.corners[i];
            lengthSoFar += Vector3.Distance(previousCorner, currentCorner);
            previousCorner = currentCorner;
            i++;
        }
        return lengthSoFar;
    }

Długość ścieżki liczy, ale jednak coś jest nie tak.
Obiekt porusza się po wyznaczonej ścieżce, jednak nie robi tego od najkrótszej do najdłuższej odległości a po kolei jak są targety ustawione w tablicy.
Dodatkowo dodałem parę zmiennych sprawdzających podgląd jak wyglądają w załączniku.