Add ext SOLID ut

diff --git a/ut/rrtext.t.cc b/ut/rrtext.t.cc
index 5fe1ab9ff7ac7a27651871d9a2a750f19d4dfb28..e884763454120c0d3cc84456cac5dada3c2c54ae 100644 (file)
--- a/ut/rrtext.t.cc
+++ b/ut/rrtext.t.cc
@@ -4,7 +4,7 @@
 #include "rrtext.h"
 #include "reeds_shepp.h"
 
-WVTEST_MAIN("RRT extensions")
+WVTEST_MAIN("RRT extensions 1")
 {
         RRTNode n1;
         RRTNode n2;
@@ -20,3 +20,40 @@ WVTEST_MAIN("RRT extensions")
         ReedsSheppStateSpace rsss(n1.mtr());
         WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001);
 }
+
+WVTEST_MAIN("RRT extensions 2")
+{
+        RRTExt2 rrts;
+        WVPASSEQ_DOUBLE(cc(rrts.nodes().front()), 0, 0.00001);
+        WVPASSEQ(rrts.nodes().size(), 1);
+        rrts.next();
+        WVPASSLT(1, rrts.nodes().size());
+        WVPASSEQ(rrts.samples().size(), 1);
+        rrts.goals().push_back(BicycleCar());
+        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);
+        Obstacle o;
+        o.poly().push_back(std::make_tuple(5, 5));
+        o.poly().push_back(std::make_tuple(6, 5));
+        o.poly().push_back(std::make_tuple(6, 6));
+        o.poly().push_back(std::make_tuple(5, 6));
+        o.poly().push_back(std::make_tuple(5, 5));
+        rrts.obstacles().push_back(o);
+        rrts.init_solid();
+        while (rrts.next()) {}
+        rrts.deinit_solid();
+        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()
+        );
+}