Add ext cost ut

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 87342fedd7cbd8fa87f7dc5055f4f7affc1f2e9d..0d00acb16d5008fcb594b765e3b56e5630b37b6b 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,6 +25,7 @@ add_executable(ut
         wvtest/cpp/wvtestmain.cc
         wvtest/cpp/wvtest.cc
         ut/rrts.t.cc
+        ut/rrtext.t.cc
 )
 target_link_libraries(ut rrts)
 

diff --git a/ut/rrtext.t.cc b/ut/rrtext.t.cc
new file mode 100644 (file)
index 0000000..5fe1ab9
--- /dev/null
+++ b/ut/rrtext.t.cc
@@ -0,0 +1,22 @@
+#include <cmath>
+#include "wvtest.h"
+
+#include "rrtext.h"
+#include "reeds_shepp.h"
+
+WVTEST_MAIN("RRT extensions")
+{
+        RRTNode n1;
+        RRTNode n2;
+        RRTExt1 e1;
+        // Test Matej's heuristics.
+        n2.h(M_PI);
+        e1.next();
+        WVPASSEQ_DOUBLE(e1.cost_search(n1, n2), M_PI * n1.mtr(), 0.00001);
+        // Test Reeds and Shepp path length.
+        n2.h(0.3);
+        double q0[] = {n1.x(), n1.y(), n1.h()};
+        double q1[] = {n2.x(), n2.y(), n2.h()};
+        ReedsSheppStateSpace rsss(n1.mtr());
+        WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001);
+}