Implement naive collide

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index e1314c224621c889f44305ba9283e2cb0e3f83a8..ad2790708fa758499039c6ece90a115cb1761ed5 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -18,6 +18,9 @@ Obstacle::Obstacle()
 // RRT procedures
 bool RRTS::collide(std::vector<std::tuple<double, double>> &poly)
 {
+        for (auto &o: this->obstacles())
+                if (std::get<0>(::collide(poly, o.poly())))
+                        return true;
         return false;
 }