From 36a442b84ffbab2ce3863ff82068f30b573809b8 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 17 Jun 2019 10:39:06 +0200
Subject: [PATCH] Fix repeating cusp in opt

When two nodes are near themselves, do not use both.
---
 base/rrtbase.cc | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

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());
-- 
2.39.2