Fix collision detection

diff --git a/src/rrts.cc b/src/rrts.cc
index d275736fae9c7fec10a1ac2be04bae7238f83bff..937160319eb6f6884add356e8eb8bde57e39ccc2 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -34,16 +34,26 @@ bool RRTS::collide_steered_from(RRTNode &f)
 {
         std::vector<std::tuple<double, double>> s;
         s.push_back(std::make_tuple(f.x(), f.y()));
-        for (auto &n: this->steered())
-                s.push_back(std::make_tuple(n.x(), n.y()));
+        for (auto &n: this->steered()) {
+                s.push_back(std::make_tuple(n.lfx(), n.lfy()));
+                s.push_back(std::make_tuple(n.lrx(), n.lry()));
+                s.push_back(std::make_tuple(n.rrx(), n.rry()));
+                s.push_back(std::make_tuple(n.rfx(), n.rfy()));
+        }
         return this->collide(s);
 }
 
 bool RRTS::collide_two_nodes(RRTNode &f, RRTNode &t)
 {
         std::vector<std::tuple<double, double>> p;
-        p.push_back(std::make_tuple(f.x(), f.y()));
-        p.push_back(std::make_tuple(t.x(), t.y()));
+        p.push_back(std::make_tuple(f.lfx(), f.lfy()));
+        p.push_back(std::make_tuple(f.lrx(), f.lry()));
+        p.push_back(std::make_tuple(f.rrx(), f.rry()));
+        p.push_back(std::make_tuple(f.rfx(), f.rfy()));
+        p.push_back(std::make_tuple(t.lfx(), t.lfy()));
+        p.push_back(std::make_tuple(t.lrx(), t.lry()));
+        p.push_back(std::make_tuple(t.rrx(), t.rry()));
+        p.push_back(std::make_tuple(t.rfx(), t.rfy()));
         return this->collide(p);
 }