fixed a bug when no obstacles exist

ZJU-FAST-Lab · May 19, 2022 · 081944d · 081944d
1 parent 5b62979
commit 081944d
Showing 1 changed file with 7 additions and 2 deletions.
diff --git a/gcopter/include/gcopter/firi.hpp b/gcopter/include/gcopter/firi.hpp
@@ -339,9 +339,14 @@ namespace firi
             nH = 0;
 
             bool completed = false;
-            int bdMinId, pcMinId;
+            int bdMinId;
             double minSqrD = distDs.minCoeff(&bdMinId);
-            double minSqrR = distRs.minCoeff(&pcMinId);
+            double minSqrR = INFINITY;
+            int pcMinId = 0;
+            if (distRs.cols() == 0)
+            {
+                minSqrR = distRs.minCoeff(&pcMinId);
+            }
             for (int i = 0; !completed && i < (M + N); ++i)
             {
                 if (minSqrD < minSqrR)