Extend naive rrts ut

diff --git a/ut/rrts.t.cc b/ut/rrts.t.cc
index 702c22033473d304e6d1fc8697d55861aff9ed98..a872cb0a1ab7de696eac0fa4072b6319988fe927 100644 (file)
--- a/ut/rrts.t.cc
+++ b/ut/rrts.t.cc
@@ -16,5 +16,22 @@ WVTEST_MAIN("RRT* basic tests")
         rrts.next();
         WVPASSEQ(rrts.nodes().size(), 2);
         WVPASSEQ(rrts.samples().size(), 1);
+        rrts.goals().push_back(RRTNode());
+        rrts.goals().back().x(10);
+        rrts.goals().back().y(10);
+        rrts.goals().back().h(0);
+        WVPASSEQ(rrts.goals().size(), 1);
+        rrts.set_sample(0, 10, 0, 10, 0, 2 * M_PI);
         while (rrts.next()) {}
+        WVPASS(rrts.path().size() > 0);
+        WVPASS(
+                rrts.nodes().size() > 0
+                && rrts.path().size() > 0
+                && &rrts.nodes().front() == rrts.path().front()
+        );
+        WVPASS(
+                rrts.goals().size() > 0
+                && rrts.path().size() > 0
+                && &rrts.goals().front() == rrts.path().back()
+        );
 }