From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 17 Jun 2019 08:39:06 +0000 (+0200)
Subject: Fix repeating cusp in opt
X-Git-Tag: v0.7.0~7^2~8
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/36a442b84ffbab2ce3863ff82068f30b573809b8?ds=sidebyside

Fix repeating cusp in opt

When two nodes are near themselves, do not use both.
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 0234fd4..130b138 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -923,7 +923,10 @@ bool RRTBase::opt_path()
                 return false;
         std::vector<RRTNode *> cusps;
         for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
-                if (tmp_cusps[i] != tmp_cusps[(i + 1) % tmp_cusps.size()])
+                if (!IS_NEAR(
+                        tmp_cusps[i],
+                        tmp_cusps[(i + 1) % tmp_cusps.size()]
+                ))
                         cusps.push_back(tmp_cusps[i]);
         }
         std::reverse(cusps.begin(), cusps.end());